package loppy.collision 
{
  import loppy.dynamics.CircleShape;
  import loppy.dynamics.Contact;
  import loppy.dynamics.ContactPoint;
  import loppy.dynamics.PolygonShape;
  import loppy.dynamics.Proxy;
  import loppy.dynamics.Shape;
  import loppy.loppy_internal;
  import loppy.math.Vec2;
  import loppy.utils.MathUtils;
  
  public class Collision 
  {
    use namespace loppy_internal;
    
    public static const CIRCLE_CIRCLE :uint = Shape.CIRCLE;
    public static const POLYGON_CIRCLE:uint = Shape.POLYGON | Shape.CIRCLE;
    
    public static function collideProxies(proxy1:Proxy, proxy2:Proxy):Boolean
    {
      return proxy1.intersects(proxy2);
    }
    
    public static function collideCircles
    (
      contactPoints:Vector.<ContactPoint>, 
      c1:CircleShape, c2:CircleShape
    ):void
    {
      var d     :Vec2        = c2.globalPosition.sub(c1.globalPosition); // points from c1 to c2
      var dLenSQ:Number      = d.lengthSQ();
      var r     :Number      = c1.radius + c2.radius;
      
      //generate contact point
      if (dLenSQ < r * r)
      {
        var contactPoint:ContactPoint = ContactPoint.pool.get();
        var dLen:Number = Math.sqrt(dLenSQ);
        
        contactPoint.normal.copy(d.normalizeThis());
        contactPoint.tangent.copy(contactPoint.normal).rotateCcwRightAngleThis();
        contactPoint.depth = r - dLen;
        contactPoint.position
          .copy(contactPoint.normal)
          .mulThis(c1.radius - contactPoint.depth * c1.radius / r)
          .addThis(c1.globalPosition);
        
        contactPoints.push(contactPoint);
      }
    }
    
    public static function collidePolyconCircle
    (
      contactPoints:Vector.<ContactPoint>, 
      p:PolygonShape, c:CircleShape
    ):void
    {
      var i:int, len:int;
      
      //compute circle in the polygon space
      var localC:Vec2 = Vec2.pool.get();
      localC.copy(c.globalPosition).subThis(p.globalPosition).rotateThis(-p.globalRotation);
      
      //find the minimum separating edge
      var separation :Number = Number.NEGATIVE_INFINITY;
      var radius     :Number = p.radius + c.radius;
      var diff       :Vec2   = Vec2.pool.get();
      var normalIndex:int    = 0;
      for (i = 0, len = p.localVertices.length; i < len; ++i)
      {
        var localVert:Vec2 = p.localVertices[i];
        var normal   :Vec2 = p.localNormals [i];
        diff.copy(localC).subThis(localVert);
        
        //separation of circle from the current edge (v[i], v[i + 1])
        var s:Number = diff.dot(normal);
        
        //early out
        if (s > radius)
        {
          Vec2.pool.put(localC);
          Vec2.pool.put(diff);
          return;
        }
        
        if (s > separation)
        {
          separation = s;
          normalIndex = i;
        }
      }
        
      //vertices of the incidental edge
      var vertIndex1:int = normalIndex;
      var vertIndex2:int = (normalIndex + 1 < p.localVertices.length)?(normalIndex + 1):(0);
      var vert1:Vec2 = p.localVertices[vertIndex1];
      var vert2:Vec2 = p.localVertices[vertIndex2];
      
      var contactPoint:ContactPoint = ContactPoint.pool.get();;
      
      //circle center is inside the polygon
      if (separation < Number.MIN_VALUE)
      {
        //TODO: accurate contact point instead of edge center
        contactPoint.normal.copy(p.globalNormals[normalIndex]);
        contactPoint.tangent.copy(contactPoint.normal).rotateCcwRightAngleThis();
        contactPoint.position
          .set
          (
            0.5 * (vert1.x + vert2.x), 
            0.5 * (vert1.y + vert2.y)
          )
          .rotateThis(p.globalRotation)
          .addThis(p.globalPosition);
      }
      //circle center is outside the polygon
      else
      {
        //dot products of (circleCenter - vert) onto the incidental edge (otherVert - vert)
        var diff1 :Vec2 = Vec2.pool.get();
        var diff2 :Vec2 = Vec2.pool.get();
        var edge12:Vec2 = Vec2.pool.get();
        var edge21:Vec2 = Vec2.pool.get();
        diff1 .set(localC.x - vert1.x, localC.y - vert1.y);
        diff2 .set(localC.x - vert2.x, localC.y - vert2.y);
        edge12.set(vert2.x - vert1.x, vert2.y - vert1.y); //from vert1 to vert2
        edge21.set(vert1.x - vert2.x, vert1.y - vert2.y); //from vert2 to vert1
        
        //circle center beyond vert1
        if (diff1.dot(edge12) <= 0.0)
        {
          var diff1LenSQ:Number = diff1.lengthSQ();
          
          //not colliding with vert1, return
          if (diff1LenSQ > c.radius * c.radius)
          {
            ContactPoint.pool.put(contactPoint);
            Vec2.pool.put(diff1 );
            Vec2.pool.put(diff2 );
            Vec2.pool.put(edge12);
            Vec2.pool.put(edge21);
            return;
          }
          
          contactPoint.position.copy(p.globalVertices[vertIndex1]);
          contactPoint.normal.copy(diff1).rotateThis(p.globalRotation).normalizeThis();
          contactPoint.tangent.copy(contactPoint.normal).rotateCwRightAngleThis();
          contactPoint.depth = c.radius - Math.sqrt(diff1LenSQ);
        }
        //circle center beyond vert2
        else if (diff2.dot(edge21) <= 0.0)
        {
          var diff2LenSQ:Number = diff2.lengthSQ();
          
          //not colliding with vert2, return
          if (diff2LenSQ > c.radius * c.radius)
          {
            ContactPoint.pool.put(contactPoint);
            Vec2.pool.put(diff1 );
            Vec2.pool.put(diff2 );
            Vec2.pool.put(edge12);
            Vec2.pool.put(edge21);
            return;
          }
          
          contactPoint.position.copy(p.globalVertices[vertIndex2]);
          contactPoint.normal.copy(diff2).rotateThis(p.globalRotation).normalizeThis();
          contactPoint.tangent.copy(contactPoint.normal).rotateCwRightAngleThis();
          contactPoint.depth = c.radius - Math.sqrt(diff2LenSQ);
        }
        //circle center above edge
        else
        {
          contactPoint.normal.copy(p.globalNormals[normalIndex]);
          contactPoint.tangent.copy(contactPoint.normal).rotateCcwRightAngleThis();
          /*
          contactPoint.position
            .set
            (
              0.5 * (vert1.x + vert2.x), 
              0.5 * (vert1.y + vert2.y)
            )
            .rotateThis(p.globalRotation)
            .addThis(p.globalPosition);
          */
          contactPoint.position
            .copy(vert1)
            .addThis(diff1.projectThis(edge12))
            .rotateThis(p.globalRotation)
            .addThis(p.globalPosition);
        }
        
        Vec2.pool.put(diff1 );
        Vec2.pool.put(diff2 );
        Vec2.pool.put(edge12);
        Vec2.pool.put(edge21);
      }
      contactPoints.push(contactPoint);
      
      Vec2.pool.put(localC);
      Vec2.pool.put(diff);
    }
  }
}